
package kuti.grp5;

import java.awt.geom.Point2D;
import java.awt.geom.Line2D;
import kuti.IKuti;

/**
 * The kuti trajectory define the total trajectory of the kuti. It detect if
 * the kuti is colliding with an obstacle.
 * 
 * @author Marc Schorderet <marc.schorderet@edu.hefr.ch>
 * @author Vincenzo Do <vincenzo.do@edu.hefr.ch>
 *
 */

public class KutiTrajectory {
    
    /**
     * The current kuti
     */
    
    private IKuti kuti;
    
    /**
     * The traveled distance
     */
    
    private double distance;
    
    /**
     * The radius of the kuti
     */
    
    private double radius;
    
    
    public KutiTrajectory(IKuti kuti, double distance, double diameter){
        
        this.kuti = kuti;
        this.distance = distance;
        this.radius = diameter / 2;
        
    }
    
    /**
     * This method returns the point on the trajectory segment that kuti 
     * collision with an object. 
     * 
     * If there is no collision with another object, 
     * this will return the last point of the trajectory segment kuti.
     * 
     * @param obstacle  The obstacle segment
     * 
     * @return Returns the point on the trajectory segment that kuti 
     * collision with an object.
     * 
     */
    
    public Line2D getIntersectionPoint(Line2D obstacle){
        
        Line2D trajectory = new Line2D.Double(kuti.getCenterPosition(), getFrontCenter(distance));
        
        Point2D intersect, impact;
        double tempDist;
        
        try{
            
            intersect = intersectionWithOnLine(obstacle);
            impact =    getCollisionPointWithALine(obstacle);
            tempDist =  intersect.distance(kuti.getCenterPosition());
            
            boolean interPtIsCorrect1 = Math.abs(obstacle.ptSegDist(intersect) - radius) < 0.1;
            boolean interPtIsCorrect2 = trajectory.ptSegDist(intersect) < 0.1;
        
            if(distance > tempDist && interPtIsCorrect1 && interPtIsCorrect2){
                
                distance = tempDist;
                return new Line2D.Double(intersect, impact);
                
            }else{
                
                throw new Exception("The kuti can't intersect the obstacle");
                
            }
            
        }catch(Exception e){
            
            double orientation = getOrientation(obstacle);
            Point2D p1 = moveTo(obstacle.getP1(), Double.MIN_VALUE, orientation);
            Point2D p2 = moveTo(obstacle.getP2(), Double.MAX_VALUE, orientation);
            Line2D bigObstacle = new Line2D.Double(p1, p2);
            
            try{
                
                intersect = intersectionWithOnLine(bigObstacle);
                
                if(Math.abs(bigObstacle.ptSegDist(intersect) - radius) > 1e-5){
                    
                   Line2D colPt = goToPoints(obstacle);
                    
                    if(colPt != null){
                        //System.out.println(" * Un point de l'obstacle se trouve sur la trajectoire du kuti");
                        distance = colPt.getP1().distance(kuti.getCenterPosition());
                        return colPt;
                    }
                    
                }else{
                    
                    Line2D colPt = goToPoints(obstacle);
                    
                    if(colPt != null){
                        distance = colPt.getP1().distance(kuti.getCenterPosition());
                        return colPt;
                    }
                    
                }
                
                
            }catch(Exception ex){
                
                Line2D colPt = goToPoints(obstacle);
                    
                if(colPt != null){
                    
                    if(colPt.getP1() != null && colPt.getP2() != null){
                        distance = colPt.getP1().distance(kuti.getCenterPosition());
                        return colPt;
                    }
                    
                }
                
                
            }
            
                    
        }
        
        distance = 0.0;
        return new Line2D.Double(kuti.getCenterPosition(), getCollisionPointWithALine(obstacle));
        
        
    }
    
    public Line2D goToPoints(Line2D obstacle){
        
        Line2D trajectory = new Line2D.Double(kuti.getCenterPosition(), getFrontCenter(distance));
        
        double tempDist = 0.0;
        Point2D intersect = null, impact = null;
        
        Point2D op1 = obstacle.getP1();
        Point2D op2 = obstacle.getP2();
                    
        boolean collidingWithPoint = false;
                    
        if(trajectory.ptSegDist(op1) < radius){
                       
            intersect = intersectionWithOnPoint(op1, radius);
            impact = getCollisionPointWithAPoint(op1, false);
            tempDist = intersect.distance(kuti.getCenterPosition());
            collidingWithPoint = true;

        }
        
        if(trajectory.ptSegDist(op2) < radius){
                       
            Point2D intersect2 = intersectionWithOnPoint(op2, radius);
            double dist = intersect2.distance(kuti.getCenterPosition());
            if(!collidingWithPoint || dist < tempDist){
                intersect = intersect2;
                impact = getCollisionPointWithAPoint(op2, false);
                collidingWithPoint = true;
            }

        }
        
        if(collidingWithPoint){
            
            double distanceIntersectObstacle = obstacle.ptSegDist(intersect);
            double precision = 1e-5;
            
            if(distanceIntersectObstacle < radius){
                return new Line2D.Double(intersect, impact);
            }else{
                if(Math.abs(distanceIntersectObstacle-radius) < precision){
                    return new Line2D.Double(intersect, impact);
                }else{
                    return null;
                }
            }
            
        }else{
            return null;
        }
        
    }
    
    
    private static boolean isPointOnSegment(Line2D segment, Point2D point){
        
        double precision = 1e-5;
        
        return segment.ptSegDist(point) < precision;
        
    }
    
    private Point2D intersectionWithOnLine(Line2D obstacle) throws Exception{
        
        // Calculate the theoric end point of the kuti when it not colliding
        Point2D endPoint = moveTo(kuti.getCenterPosition(), Double.MAX_VALUE, kuti.getOrientation());
        
        // Create a segment with the trajectory of the kuti
        Line2D trajectory = new Line2D.Double(kuti.getCenterPosition(), endPoint);
        
        // Calculate the minimal angle between two segments
        double alpha = minAngleBetween(trajectory, obstacle);
        
        // Calculate the intersection point of the kuti and the obstacle
        Point2D intersection = intersectionLineLine(trajectory, obstacle);
        
        if(intersection == null){
            
            throw new Exception("There are no intersect point.");
            
        }
        
        // Calculate the distance between the intersection point and the colliding point
        double r = radius;
        double a = r / Math.sin(alpha);
        double b = a * Math.cos(alpha);
        
        double dist = intersection.distance(kuti.getCenterPosition()) - a;
        
        if(dist > distance || dist < 0.0){
            
            throw new Exception("The distance is not valide [" + dist + "]");
            
        }
        
        return moveTo(kuti.getCenterPosition(), dist, kuti.getOrientation());
        
    }
    
    public double minAngleBetween(Line2D l1, Line2D l2){
        
        double o1 = getOrientation(l1);
        double o2 = getOrientation(l2);
        
        double res = Math.abs(o1 - o2);
        if(res > Math.PI/2)
            res = Math.PI - res;
        
        return res;
    }
    
    private Point2D intersectionWithOnPoint(Point2D nearestPoint, double radius){
        
        if(distance == 0.0){
            return kuti.getCenterPosition();
        }
        
        Point2D maxEndPoint = moveTo(kuti.getCenterPosition(), distance, kuti.getOrientation());
        
        Vector2D vKutiTrajectory = new Vector2D(kuti.getCenterPosition(), maxEndPoint);
        Vector2D vKutiObstacle = new Vector2D(kuti.getCenterPosition(), nearestPoint);
        
        Vector2D vProjection = Vector2D.projection(vKutiTrajectory, vKutiObstacle);
        vProjection.add(new Vector2D(kuti.getCenterPosition()));
        
        Point2D limitPoint = vProjection.getPoint();
        
        double b = limitPoint.distance(nearestPoint);
        double c = radius;        
        double a = Math.sqrt(c*c - b*b);
        
        if(Double.isNaN(a)){
            //System.out.println("J'avance pas 3");
            return kuti.getCenterPosition();
        }
        
        double collidingDist = kuti.getCenterPosition().distance(limitPoint) - a;
        
        if(collidingDist <= distance && collidingDist > 0.0){
            return moveTo(kuti.getCenterPosition(), collidingDist, kuti.getOrientation());
        }else{
            //System.out.println("J'avance pas 2");
            return kuti.getCenterPosition();
        }
        
    }
    
    public Point2D nearestPoint(Point2D start, Line2D line){
        
        double distP1 = start.distance(line.getP1());
        double distP2 = start.distance(line.getP2());
        
        return (distP1 < distP2) ? line.getP1() : line.getP2();
        
    }
    
    /**
     * This method returns the point on the trajectory of the kuti, wherein the 
     * distance between it and another kuti is exactly the length of the 
     * diameter. This simulates a collision between two kuti.
     * 
     * @param otherKuti other kuti
     * 
     * @return Returns the center point of the collading kuti
     */
    
    public Line2D getIntersectionPoint(IKuti otherKuti){
        
        /*if(distance == 0.0){
            return kuti.getCenterPosition();
        }*/
        
        
        Point2D intersect = intersectionWithOnPoint(otherKuti.getCenterPosition(), 2 * radius);
        Point2D q = getCollisionPointWithAPoint(kuti.getCenterPosition(), true);
        double temp = intersect.distance(kuti.getCenterPosition());
        
        //return new Line2D.Double(kuti.getCenterPosition(), kuti.getCenterPosition());
        
        if(distance > temp && otherKuti.getCenterPosition().distance(intersect) >= radius){
            distance = temp;
            return new Line2D.Double(intersect, q);
        }else{
            distance = 0.0;
            //System.out.println("J'avance pas 1");
            return new Line2D.Double(kuti.getCenterPosition(), q);
        }
        
        
        
    }
    
    // ---------------------------------------------------------------------------------------------------------------------
    
    
    /**
     * This method returns the intersection point between two segments.
     * 
     * If the two segments have no intersection points, the method return null.
     * 
     * @param kuti      First segment
     * @param obstacle  Second segment
     * 
     * @return Returns the intersection point between two segments.
     */
    
    
    public static Point2D intersectionLineLine(Line2D kuti, Line2D obstacle){
        
        if(!kuti.intersectsLine(obstacle))
            return null;
        
        double orientationKuti = getOrientation(kuti);
        double orientationObstacle = getOrientation(obstacle);
        double delta = 1e-6;
        
        if(orientationKuti <= delta){
            
            if(orientationObstacle <= delta){
                
                if(kuti.getP1().distance(obstacle.getP1()) < kuti.getP1().distance(obstacle.getP2()))
                    return (Point2D) obstacle.getP1().clone();
                return (Point2D) obstacle.getP2().clone();
                
            }else if(Math.abs(orientationObstacle - (Math.PI / 2)) <= delta){
                
                return new Point2D.Double(obstacle.getX1(), kuti.getY1());
                
            }else{
                
                double a = Math.tan(orientationObstacle);
                double b = obstacle.getY1() - (a * obstacle.getX1());
                
                double y = kuti.getY1();
                double x = (y - b) / a;
                
                return new Point2D.Double(x, y);
                
            }
            
        }else if(Math.abs(orientationKuti - (Math.PI / 2)) <= delta){
            
            if(orientationObstacle <= delta){
                
                return new Point2D.Double(kuti.getX1(), obstacle.getY1());
                
            }else if(Math.abs(orientationObstacle - (Math.PI / 2)) <= delta){
                
                if(kuti.getP1().distance(obstacle.getP1()) < kuti.getP1().distance(obstacle.getP2()))
                    return (Point2D) obstacle.getP1().clone();
                return (Point2D) obstacle.getP2().clone();
                
            }else{
                
                double a = Math.tan(orientationObstacle);
                double b = obstacle.getY1() - (a * obstacle.getX1());
                
                double x = kuti.getX1();
                double y = a * x + b;
                
                return new Point2D.Double(x, y);
                
            }
             
            
        }else{
    
            double aKuti = Math.tan(getOrientation(kuti));
            double x1Kuti = kuti.getX1();
            double y1Kuti = kuti.getY1();
            double bKuti = y1Kuti - (aKuti * x1Kuti);
           
            double aObject = Math.tan(getOrientation(obstacle));
            double x1Object = obstacle.getX1();
            double y1Object = obstacle.getY1();
            double bObject = y1Object - (aObject * x1Object);
           
            double xCommun = (bKuti - bObject) / (aObject - aKuti);
            double yCommun = aKuti * xCommun + bKuti;
           
            return new Point2D.Double(xCommun, yCommun);
        
        }
    
    }
    
    /**
    * This method calculate the orientation of a segment
    * 
    * @param seg    Segment
    * 
    * @return Returns the orientation of a segment [0 to Math.PI[ 
    */
    
    public static double getOrientation(Line2D seg) {
        double angle = Math.atan2(getDifferenceOnYAxis(seg), getDifferenceOnXAxis(seg));
        angle += (angle < 0) ? Math.PI : 0;
        return angle;
    }
    
    /**
     * This method check if the kuti trajectory intersects with an obstacle (Line2D).
     * 
     * @param obstacle  The obstacle to check.
     * 
     * @return Returns true if the kuti trajectory intersects with the obstable,
     * else returns false.
     */
    
    public boolean intersectsLine(Line2D obstacle){
    
        
        Line2D leftWall = getLeftWall();
        Line2D rightWall = getRightWall();
        Line2D frontWall = new Line2D.Double(leftWall.getP2(), rightWall.getP2());
        Point2D frontCenter = getFrontCenter(distance);
        
        double precision = 1e-4;
        
        // Ckeck if the first or second point of the obstable is in the rectangle.
        if(isInRectangle(obstacle.getP1(), leftWall, rightWall) ||
                isInRectangle(obstacle.getP2(), leftWall, rightWall)){
            //System.out.println("Cas 1");
            return true;
        }
        
        
        // Ckeck if the obstacle intersect with the left, right or front of the
        // rectangle.
        if(frontWall.intersectsLine(obstacle)){
            
            if(leftWall.intersectsLine(obstacle)){
                
                double distanceToWallP1 = obstacle.ptSegDist(leftWall.getP1());
                double distanceToWallP2 = obstacle.ptSegDist(leftWall.getP2());
                if (distanceToWallP1 > precision || distanceToWallP2 > precision) {
                    //System.out.println("Cas 2 - " + Math.abs(distanceToWallP1 - radius) + ", " + Math.abs(distanceToWallP2 - radius));
                    return true;

                }
            }
            
            if(rightWall.intersectsLine(obstacle)){
                
                double distanceToWallP1 = obstacle.ptSegDist(rightWall.getP1());
                double distanceToWallP2 = obstacle.ptSegDist(rightWall.getP2());
                if (distanceToWallP1 > precision || distanceToWallP2 > precision) {
                    //System.out.println("Cas 3 - " + Math.abs(distanceToWallP1 - radius) + ", " + Math.abs(distanceToWallP2 - radius));
                    return true;
                }
                
            }
            
        }
        
        if(leftWall.intersectsLine(obstacle) && rightWall.intersectsLine(obstacle)){
            //System.out.println("Cas 4");
            return true;
        }
        
        if(obstacle.ptSegDist(frontCenter) < radius){
            
            if(obstacle.getP1().distance(frontCenter) < radius
                    || obstacle.getP2().distance(frontCenter) < radius){
                //System.out.println("Cas 5");
                return true;
            }
            if((Math.abs(obstacle.ptSegDist(frontCenter) - radius )) > precision){
            //System.out.println("Cas 6 - " + (Math.abs(obstacle.ptSegDist(frontCenter) - radius )));
            //if(Math.abs(obstacle.ptSegDist(frontCenter) - radius) < 1e-5){
                return true;
            //}
            }
            
        }
                
        return false;
        
    }
    
    /**
     * This method ckeck if two kutis colliding.
     * 
     * @param other The other kuti.
     * 
     * @return Returns true if the two kutis colliding, else returns false.
     */
    
    public boolean intersectsKuti(IKuti other){
        
        Line2D leftWall = getLeftWall();
        if(leftWall.ptSegDist(other.getCenterPosition()) < radius){
            return true;
        }
        
        Line2D rightWall = getRightWall();
        if(rightWall.ptSegDist(other.getCenterPosition()) < radius){
            return true;
        }
        
        if(getFrontCenter(distance).distance(other.getCenterPosition()) < radius * 2){
            return true;
        }
        
        return false;
        
    }
    
    /**
     * This method returns the center of the kuti, when it moves the distance 
     * specified as a parameter.
     * 
     * @param distance  The distance to move
     * 
     * @return Returns the center of the kuti after moving.
     */
    
    public Point2D getFrontCenter(double distance){
        
        return moveTo(kuti.getCenterPosition(), distance, kuti.getOrientation());
        
    }
    
    /**
     * This method returns a segment representing the left wall of the 
     * trajectory of the kuti.
     * 
     * @return Returns the left wall of the trajectory of the kut.
     */
    
    public Line2D getLeftWall(){
        
        Point2D leftP1 = moveTo(kuti.getCenterPosition(), radius, rotationLeft(kuti.getOrientation(), Math.PI/2));
        Point2D leftP2 = moveTo(leftP1, distance, kuti.getOrientation());
        
        return new Line2D.Double(leftP1, leftP2);
        
    }
    
    /**
     * This method returns a segment representing the right wall of the 
     * trajectory of the kuti.
     * 
     * @return Returns the right wall of the trajectory of the kut.
     */
    
    public Line2D getRightWall(){
        
        Point2D rightP1 = moveTo(kuti.getCenterPosition(), radius, rotationRight(kuti.getOrientation(), Math.PI/2));
        Point2D rightP2 = moveTo(rightP1, distance, kuti.getOrientation());
        
        return new Line2D.Double(rightP1, rightP2);
        
    }
    
    
    /**
     * Moving the center point of the kuti.
     *
     * @param point The start point (is modified)
     * @param distance The moving distance
     * @param orientation The kuti orientation
     * 
     * @return Returns the point after moving.
     */
    
    public static Point2D moveTo(Point2D point, double distance, double orientation) {

        double distanceX = distance * Math.cos(orientation);
        double distanceY = distance * Math.sin(orientation);

        return new Point2D.Double(point.getX() + distanceX, point.getY() + distanceY);

    }
    
    /**
     * This method calculate the new orientation angle of a right rotation.
     * 
     * The value is returned in Radian. [0...2*Math.Pi[
     * 
     * @param orientation   Initial orientation  (Radian)
     * @param angle         Angle of rotation    (Radian)
     * 
     * @return Returns the new orientation of a right rotation
     */
    
    public static double rotationRight(double orientation, double angle){
        
        orientation -= angle;
        while(orientation < 0)
            orientation += 2 * Math.PI;
        
        orientation %= (2 * Math.PI);
        return orientation;
        
    }
    
    /**
     * This method calculate the new orientation angle of a left rotation.
     * 
     * The value is returned in Radian. [0...2*Math.Pi[
     * 
     * @param orientation   Initial orientation  (Radian)
     * @param angle         Angle of rotation    (Radian)
     * 
     * @return Returns the new orientation of a left rotation
     */
    
    public static double rotationLeft(double orientation, double angle){
        orientation += angle;
        while(orientation < 0)
            orientation += 2 * Math.PI;
        
        orientation %= (2 * Math.PI);
        return orientation;
    }
    
    
    public static double signedArea(Point2D p1, Point2D p2, Point2D p3) {
        return (p2.getX() - p1.getX()) * (p3.getY() - p1.getY()) - (p3.getX() - p1.getX()) * (p2.getY() - p1.getY());
    }
    
    
    /**
     * This methode check if three points turn in counter clockwise.
     * 
     * @param p1    the first point (start)
     * @param p2    the second point (angle)
     * @param p3    the thrid point (end)
     * 
     * @return Returns -1 if the point is in left angle, returns +1 if the
     * point is in right angle, or return 0 if the point is on the line.
     * 
     */
    
    public static int ccw(Point2D p1, Point2D p2, Point2D p3) {
        double a = signedArea(p1, p2, p3);
        if (a < 0)
            return -1;
        if (a > 0)
            return +1;
        return 0;
    }
    
    /**
     * This method check if a point is in the left angle of three others points.
     * 
     * @param point The point to check
     * @param a     The first point (start)
     * @param b     The second point (angle)
     * @param c     The third point (end)
     * 
     * @return Returns true if the point is in the left angle, 
     * else returns false.
     */
    
    public static boolean isInLeftAngle(Point2D point, Point2D a, Point2D b, Point2D c) {
    
        int abc = ccw(a, b, c);
        int abq = ccw(a, b, point);
        int bcq = ccw(b, c, point);

        if (abc >= 0) {
            return (abq > 0 && bcq > 0);
        } else {
            return (abq > 0 || bcq > 0);
        }
    }
    
    /**
     * This method check if a point is in a rectangle. The rectangle is specified
     * with two Lines. (Left wall and right wall)
     * 
     * @param point the point to check
     * @param left  left wall of the rectangle
     * @param right right wall of the rectangle
     * 
     * @return Returns true if the point is in or on the rectangle, 
     * else returns false.
     */
    
    public static boolean isInRectangle(Point2D point, Line2D left, Line2D right){
        
        Point2D a = right.getP1();
        Point2D b = right.getP2();
        Point2D c = left.getP2();
        Point2D d = left.getP1();
        
        if(!isInLeftAngle(point, a, b, c))
            return false;
        
        if(!isInLeftAngle(point, b, c, d))
            return false;
        
        if(!isInLeftAngle(point, c, d, a))
            return false;
        
        if(!isInLeftAngle(point, d, a, b))
            return false;
        
        return true;
        
    }
    
    
    /**
     * This method returns the difference on the X axis.
     * 
     * @param p1    First point on the line
     * @param p2    Second point on the line
     * 
     * @return Returns the difference on the X axis
     */
    
    public static double getDifferenceOnXAxis(Point2D p1, Point2D p2){
       return p2.getX() - p1.getX();
   }
    
    /**
     * This method returns the difference on the Y axis.
     * 
     * @param p1    First point on the line
     * @param p2    Second point on the line
     * 
     * @return Returns the difference on the Y axis
     */
   
   public static double getDifferenceOnYAxis(Point2D p1, Point2D p2){
       return p2.getY() - p1.getY();
   }
   
   /**
     * This method returns the difference on the X axis.
     * 
     * @param seg    Segment
     * 
     * @return Returns the difference on the X axis
     */
   
   public static double getDifferenceOnXAxis(Line2D seg){
       return getDifferenceOnXAxis(seg.getP2(), seg.getP1());
   }
   
   
   /**
     * This method returns the difference on the Y axis.
     * 
     * @param seg    Segment
     * 
     * @return Returns the difference on the Y axis
     */
   
   public static double getDifferenceOnYAxis(Line2D seg){
       return getDifferenceOnYAxis(seg.getP2(), seg.getP1());
   }
   
   private Point2D getCollisionPointWithAPoint(Point2D point, boolean withKuti){
       if(!withKuti)
           return point;
       Vector2D a = new Vector2D(kuti.getCenterPosition());
       Vector2D b = new Vector2D(point);
       b.invert();
       b.add(a);
       b.stretch(0.5);
       b.add(new Vector2D(point));
       return b.getPoint();
   }
   
   private Point2D getCollisionPointWithALine(Line2D obstacle){
       
       Vector2D a = new Vector2D(obstacle.getP1(), obstacle.getP2());
       Vector2D b = new Vector2D(obstacle.getP1(), kuti.getCenterPosition());
       
       Vector2D proj = Vector2D.projection(a, b);
       proj.add(new Vector2D(obstacle.getP1()));
       
       
       return proj.getPoint();
   }
    
    
}
